Updated!

I get this code from this site

It's A* Search Algorithm(finding shortest path with heuristics)

I modify most of variable names and some if conditions from the original version to satisfy my syntactic taste.

It works in C++ (as I can't see any trouble with it) but fails in Java version.

Java

Code:

String findPath(int startX, int startY, int finishX, int finishY) {

    @SuppressWarnings("unchecked")
    LinkedList<Node>[] nodeList = (LinkedList<Node>[]) new LinkedList<?>[2];

    nodeList[0] = new LinkedList<Node>();
    nodeList[1] = new LinkedList<Node>();

    Node n0;
    Node m0;

    int nlIndex = 0; // queueList index

    // reset the node maps
    for(int y = 0;y < ROW_COUNT; ++y) {
        for(int x = 0;x < COL_COUNT; ++x) {
            close_nodes_map[y][x] = 0;
            open_nodes_map[y][x] = 0;
        }
    }

    // create the start node and push into list of open nodes
    n0 = new Node( startX, startY, 0, 0 );
    n0.updatePriority( finishX, finishY );
    nodeList[nlIndex].push( n0 );

    open_nodes_map[startY][startX] = n0.getPriority(); // mark it on the open nodes map

    // A* search
    while( !nodeList[nlIndex].isEmpty() )
    {
        LinkedList<Node> pq = nodeList[nlIndex];
        // get the current node w/ the highest priority
        // from the list of open nodes
        n0 = new Node( pq.peek().getX(), pq.peek().getY(), 
                       pq.peek().getIterCount(), pq.peek().getPriority());

        int x = n0.getX();
        int y = n0.getY();

        nodeList[nlIndex].pop(); // remove the node from the open list
        open_nodes_map[y][x] = 0;
        // mark it on the closed nodes map
        close_nodes_map[y][x] = 1;

        // quit searching when the goal state is reached
        //if((*n0).estimate(finishX, finishY) == 0)
        if( x == finishX && y == finishY ) 
        {
            // generate the path from finish to start
            // by following the directions
            String path = "";

            while( !( x == startX && y == startY) )
            {
                int j = dir_map[y][x];

                int c = '0' + ( j + Node.DIRECTION_COUNT / 2 ) % Node.DIRECTION_COUNT;

                path = (char)c + path;

                x += DIR_X[j];
                y += DIR_Y[j];

            }

            return path;
        }

        // generate moves (child nodes) in all possible directions
        for(int i = 0; i < Node.DIRECTION_COUNT; ++i)
        {
            int xdx = x + DIR_X[i];
            int ydy = y + DIR_Y[i];


            // boundary check
            if (!(xdx >= 0 && xdx < COL_COUNT
             &&   ydy >= 0 && ydy < ROW_COUNT)) continue;

            if ( ( gridMap.getData( ydy, xdx ) == GridMap.WALKABLE
                || gridMap.getData( ydy, xdx ) == GridMap.FINISH)
                && close_nodes_map[ydy][xdx] != 1 )
            {
                // generate a child node
                m0 = new Node( xdx, ydy, n0.getIterCount(), n0.getPriority() );
                m0.nextLevel( i );
                m0.updatePriority( finishX, finishY );

                // if it is not in the open list then add into that
                if( open_nodes_map[ydy][xdx] == 0 )
                {
                    open_nodes_map[ydy][xdx] = m0.getPriority();
                    nodeList[nlIndex].push( m0 );
                    // mark its parent node direction
                    dir_map[ydy][xdx] = ( i + Node.DIRECTION_COUNT / 2 ) % Node.DIRECTION_COUNT;
                }
                else if( open_nodes_map[ydy][xdx] > m0.getPriority() )
                {
                    // update the priority info
                    open_nodes_map[ydy][xdx] = m0.getPriority();
                    // update the parent direction info
                    dir_map[ydy][xdx] = ( i + Node.DIRECTION_COUNT / 2 ) % Node.DIRECTION_COUNT;

                    // replace the node
                    // by emptying one queueList to the other one
                    // except the node to be replaced will be ignored
                    // and the new node will be pushed in instead
                    while( !(nodeList[nlIndex].peek().getX() == xdx && 
                             nodeList[nlIndex].peek().getY() == ydy ) )
                    {                
                        nodeList[1 - nlIndex].push( nodeList[nlIndex].pop() );
                    }

                    nodeList[nlIndex].pop(); // remove the wanted node

                    // empty the larger size queueList to the smaller one
                    if( nodeList[nlIndex].size() > nodeList[ 1 - nlIndex ].size() )
                        nlIndex = 1 - nlIndex;

                    while( !nodeList[nlIndex].isEmpty() )
                    {                
                        nodeList[1 - nlIndex].push( nodeList[nlIndex].pop() );
                    }

                    nlIndex = 1 - nlIndex;
                    nodeList[nlIndex].push( m0 ); // add the better node instead
                }

            }
        }

    }

    return ""; // no route found
}

Output1:

Legends

• . = PATH
• ? = START
• X = FINISH
• 3,2,1 = OBSTACLES

(Misleading path)

Output2:

Changing these lines:

n0 = new Node( a, b, c, d );
m0 = new Node( e, f, g, h );

to

n0.set( a, b, c, d );
m0.set( e, f, g, h );

I get

(I'm really confused)

C++

Code:

std::string A_Star::findPath(int startX, int startY, int finishX, int finishY)
{
    typedef std::queue<Node> List_Container;

    List_Container nodeList[2]; // list of open (not-yet-tried) nodes
    Node n0;
    Node m0;

    int pqIndex = 0; // nodeList index

    // reset the node maps
    for(int y = 0;y < ROW_COUNT; ++y)
    {
        for(int x = 0;x < COL_COUNT; ++x)
        {
            close_nodes_map[y][x] = 0;
            open_nodes_map[y][x] = 0;
        }
    }

    // create the start node and push into list of open nodes
    n0 = Node( startX, startY, 0, 0 );
    n0.updatePriority( finishX, finishY );
    nodeList[pqIndex].push( n0 );

    open_nodes_map[startY][startX] = n0.getPriority(); // mark it on the open nodes map

    // A* search
    while( !nodeList[pqIndex].empty() )
    {
        List_Container &pq = nodeList[pqIndex];
        // get the current node w/ the highest priority
        // from the list of open nodes
        n0 = Node( pq.front().getX(), pq.front().getY(), 
                   pq.front().getIterCount(), pq.front().getPriority());

        int x = n0.getX();
        int y = n0.getY();

        nodeList[pqIndex].pop(); // remove the node from the open list
        open_nodes_map[y][x] = 0;
        // mark it on the closed nodes map
        close_nodes_map[y][x] = 1;

        // quit searching when the goal state is reached
        //if((*n0).estimate(finishX, finishY) == 0)
        if( x == finishX && y == finishY ) 
        {
            // generate the path from finish to start
            // by following the directions
            std::string path = "";

            while( !( x == startX && y == startY) )
            {
                int j = dir_map[y][x];

                char c = '0' + ( j + DIRECTION_COUNT / 2 ) % DIRECTION_COUNT;

                path = c + path;

                x += DIR_X[j];
                y += DIR_Y[j];
            }

            return path;
        }

        // generate moves (child nodes) in all possible directions
        for(int i = 0; i < DIRECTION_COUNT; ++i)
        {
            int xdx = x + DIR_X[i];
            int ydy = y + DIR_Y[i];

            // boundary check
            if (!( xdx >= 0 && xdx < COL_COUNT
                && ydy >= 0 && ydy < ROW_COUNT)) continue;

            if ( ( pGrid->getData(ydy,xdx) == WALKABLE
                || pGrid->getData(ydy, xdx) == FINISH)
                && close_nodes_map[ydy][xdx] != 1 )
            {
                // generate a child node
                m0 = Node( xdx, ydy, n0.getIterCount(), n0.getPriority() );
                m0.nextLevel( i );
                m0.updatePriority( finishX, finishY );

                // if it is not in the open list then add into that
                if( open_nodes_map[ydy][xdx] == 0 )
                {
                    open_nodes_map[ydy][xdx] = m0.getPriority();
                    nodeList[pqIndex].push( m0 );
                    // mark its parent node direction
                    dir_map[ydy][xdx] = ( i + DIRECTION_COUNT / 2 ) % DIRECTION_COUNT;
                }
                else if( open_nodes_map[ydy][xdx] > m0.getPriority() )
                {
                    // update the priority info
                    open_nodes_map[ydy][xdx] = m0.getPriority();
                    // update the parent direction info
                    dir_map[ydy][xdx] = ( i + DIRECTION_COUNT / 2 ) % DIRECTION_COUNT;

                    // replace the node
                    // by emptying one nodeList to the other one
                    // except the node to be replaced will be ignored
                    // and the new node will be pushed in instead
                    while ( !( nodeList[pqIndex].front().getX() == xdx && 
                               nodeList[pqIndex].front().getY() == ydy ) )
                    {                
                        nodeList[1 - pqIndex].push( nodeList[pqIndex].front() );
                        nodeList[pqIndex].pop();       
                    }

                    nodeList[pqIndex].pop(); // remove the wanted node

                    // empty the larger size nodeList to the smaller one
                    if( nodeList[pqIndex].size() > nodeList[ 1 - pqIndex ].size() )
                        pqIndex = 1 - pqIndex;

                    while( !nodeList[pqIndex].empty() )
                    {                
                        nodeList[1-pqIndex].push(nodeList[pqIndex].front());
                        nodeList[pqIndex].pop();
                    }

                    pqIndex = 1 - pqIndex;
                    nodeList[pqIndex].push( m0 ); // add the better node instead
                }

            }
        }

    }

    return ""; // no route found
}

Output:

Legends

• . = PATH
• ? = START
• X = FINISH
• 3,2,1 = OBSTACLES

(Just right)

From what I read about Java's documentation, I came up with the conclusion:

• C++'s std::queue<T>::front() == Java's LinkedList<T>.peek()
• Java's LinkedList<T>.pop() == C++'s std::queue<T>::front() + std::queue<T>::pop()

What might I be missing in my Java version? In what way does it became different algorithmically from the C++ version?